import CONSTANT from '../../../constant';

export default class Boundary {
    constructor(maxRows) {
        this.iBounded = new Int8Array(maxRows);
        this.iUnbounded = new Int8Array(maxRows);
        this.signs = new Int8Array(maxRows);
        this.b = new Float64Array(maxRows);
        this.numBounded = 0;
        this.numUnbounded = 0;
        this.matrixId = 0;
    }
    init(buildInfo) {
        this.numBounded = buildInfo.numBounded;
        let _g = 0, _g1 = this.numBounded;
        while(_g < _g1) {
            let i = _g++;
            this.iBounded[i] = buildInfo.iBounded[i];
            this.signs[i] = buildInfo.signs[i];
        }
        this.numUnbounded = buildInfo.numUnbounded;
        this.matrixId = 0;
        let _g2 = 0, _g3 = this.numUnbounded;
        while(_g2 < _g3) {
            let i = _g2++;
            let idx = buildInfo.iUnbounded[i];
            this.iUnbounded[i] = idx;
            this.matrixId |= 1 << idx;
        }
    }
    computeImpulses(info,mass,relVels,impulses,dImpulses,impulseFactor,noCheck) {
        const MSE=CONSTANT.SETTING_DIRECT_MLCP_SOLVER_EPS;
        let _g = 0, _g1 = this.numUnbounded;
        while(_g < _g1) {
            let idx = this.iUnbounded[_g++];
            let row = info.rows[idx];
            this.b[idx] = row.rhs * impulseFactor - relVels[idx] - row.cfm * impulses[idx];
        }
        let invMassWithoutCfm = mass._invMassWithoutCfm;
        let _g2 = 0, _g3 = this.numBounded;
        while(_g2 < _g3) {
            let i = _g2++;
            let idx = this.iBounded[i];
            let sign = this.signs[i];
            let row = info.rows[idx];
            let dImpulse = (sign < 0 ? row.minImpulse : sign > 0 ? row.maxImpulse : 0) - impulses[idx];
            dImpulses[idx] = dImpulse;
            if(dImpulse !== 0) {
                let _g = 0, _g1 = this.numUnbounded;
                while(_g < _g1) {
                    let idx2 = this.iUnbounded[_g++];
                    this.b[idx2] -= invMassWithoutCfm[idx][idx2] * dImpulse;
                }
            }
        }
        let indices = this.iUnbounded;
        let n = this.numUnbounded;
        let id = 0;
        let _g4 = 0;
        while(_g4 < n) id |= 1 << indices[_g4++];
        let massMatrix;
        if(mass._cacheComputed[id]) {
            massMatrix = mass._cachedSubmatrices[id];
        } else {
            mass.computeSubmatrix(id,indices,n);
            mass._cacheComputed[id] = true;
            massMatrix = mass._cachedSubmatrices[id];
        }
        let ok = true;
        let _g5 = 0, _g6 = this.numUnbounded;
        while(_g5 < _g6) {
            let i = _g5++;
            let idx = this.iUnbounded[i];
            let row = info.rows[idx];
            let oldImpulse = impulses[idx];
            let impulse = oldImpulse;
            let _g = 0, _g1 = this.numUnbounded;
            while(_g < _g1) {
                let j = _g++;
                impulse += this.b[this.iUnbounded[j]] * massMatrix[i][j];
            }
            if(impulse < row.minImpulse - MSE || impulse > row.maxImpulse + MSE) {
                ok = false;
                break;
            }
            dImpulses[idx] = impulse - oldImpulse;
        }
        if(noCheck) {
            return true;
        }
        if(!ok) {
            return false;
        }
        let _g7 = 0, _g8 = this.numBounded;
        while(_g7 < _g8) {
            let i = _g7++;
            let idx = this.iBounded[i];
            let row = info.rows[idx];
            let sign = this.signs[i];
            let error = 0;
            let newImpulse = impulses[idx] + dImpulses[idx];
            let relVel = relVels[idx];
            let _g = 0, _g1 = info.numRows;
            while(_g < _g1) {
                let j = _g++;
                relVel += invMassWithoutCfm[idx][j] * dImpulses[j];
            }
            error = row.rhs * impulseFactor - relVel - row.cfm * newImpulse;
            if(sign < 0 && error > MSE || sign > 0 && error < -MSE) {
                ok = false;
                break;
            }
        }
        return ok;
    }
}